#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "ballhoop definitions.h"
task main()
{

int extbatteryAvg = externalBatteryAvg;
int nxtBatteryAvg = nAvgBatteryLevel;

nxtDisplayString(3, "12V %d", extbatteryAvg);
nxtDisplayString(5, "NXT %d", nxtBatteryAvg);


servo[servo1] = CLAMP_HOME;
wait10Msec(100);
servo[servo1] = CLAMP_OPEN;
wait10Msec(100);
servo[servo1] = CLAMP_HOME;
wait10Msec(150);

servo[servo2] = 0;//lift
wait10Msec(50);
servo[servo2] = 200;
wait10Msec(100);
servo[servo2] = 127;//stop

servo[servo3] = 0;//spin
wait10Msec(50);
servo[servo3] = 200;
wait10Msec(100);
servo[servo3] = 127;//stop

servo[servo4] = BALLHOOP_HOME;
wait10Msec(100);
servo[servo4] = BALLHOOP_HOME + BALLHOOP_2FLAT;
wait10Msec(100);
servo[servo4] = BALLHOOP_HOME + BALLHOOP_RAMP;
wait10Msec(100);
servo[servo4] = BALLHOOP_HOME;
wait10Msec(150);

while(true){}
}
